#include "mcp2515.h"

uint8_t mcpMode;     // Current controller mode

void MCP2515_Pin_init() {
    // This example will use SPI0 at 1MHz.
    spi_init(SPI_PORT, 8*1000 * 1000);
    gpio_set_function(MCP2515_SPI_CLK, GPIO_FUNC_SPI);
    gpio_set_function(MCP2515_SPI_MOSI, GPIO_FUNC_SPI);
    gpio_set_function(MCP2515_SPI_MISO, GPIO_FUNC_SPI);
    // Make the SPI pins available to picotool
    bi_decl(bi_3pins_with_func(MCP2515_SPI_MISO, MCP2515_SPI_MOSI, MCP2515_SPI_CLK, GPIO_FUNC_SPI));
    gpio_init(MCP2515_SPI_CS);
    gpio_set_dir(MCP2515_SPI_CS, GPIO_OUT);
    gpio_put(MCP2515_SPI_CS, 1);
    // Make the CS pin available to picotool
    bi_decl(bi_1pin_with_name(MCP2515_SPI_CS, "SPI CS"));
}


uint8_t mcp2515_init(uint16_t canSpeed, uint8_t clock) {
    uint8_t res;
    mcp2515_reset();
    res = mcp2515_setCANCTRL_Mode(MODE_CONFIG);
    if (res > 0) {
#if DEBUG_EN
        printf("Enter setting mode fail\n");
#else
        sleep_us(10);
#endif
        return res;
    }
#if DEBUG_EN
    printf("Enter setting mode success \n");
#else
    sleep_us(10);
#endif
    // set boadrate
    if (mcp2515_configRate(canSpeed, clock)) {
#if DEBUG_EN
        printf("set rate fall!!\n");
#else
        sleep_us(10);
#endif
        return res;
    }
#if DEBUG_EN
    printf("set rate success!!\n");
#else
    sleep_us(10);
#endif
    if (res == MCP2515_OK) {
        // init canbuffers
        mcp2515_initCANBuffers();
        // interrupt mode
        mcp2515_setRegister(MCP_CANINTE, MCP_RX0IF | MCP_RX1IF);
#if (DEBUG_RXANY == 1)
        // enable both receive-buffers to receive any message and enable rollover
        mcp2515_modifyRegister(MCP_RXB0CTRL,
                               MCP_RXB_RX_MASK | MCP_RXB_BUKT_MASK,
                               MCP_RXB_RX_ANY | MCP_RXB_BUKT_MASK);
        mcp2515_modifyRegister(MCP_RXB1CTRL, MCP_RXB_RX_MASK,
                               MCP_RXB_RX_ANY);
#else
        // enable both receive-buffers to receive messages with std. and ext. identifiers and enable rollover
        mcp2515_modifyRegister(MCP_RXB0CTRL,
                               MCP_RXB_RX_MASK | MCP_RXB_BUKT_MASK | MCP_RTR_MASK,
                               MCP_RXB_RX_STDEXT | MCP_RXB_BUKT_MASK);
        mcp2515_modifyRegister(MCP_RXB1CTRL, MCP_RXB_RX_MASK,
                               MCP_RXB_RX_STDEXT);
#endif
        // enter normal mode
        res = setMode(MODE_NORMAL);
        if (res) {
#if DEBUG_EN
            printf("Enter Normal Mode Fail!!\n");
#else
            sleep_us(10);
#endif
            return res;
        }
#if DEBUG_EN
        printf("Enter Normal Mode Success!!\n");
#else
        sleep_us(10);
#endif
    }
    return res;
}

void mcp2515_reset(void) {
    MCP2515_SELECT();
    uint8_t buf = MCP_RESET;
    spi_write_blocking(SPI_PORT, &buf, 1);
    sleep_ms(10);
    MCP2515_UNSELECT();
#if DEBUG_EN
    printf("reset done\n");
#endif
    sleep_ms(30);
}

uint8_t mcp2515_setCANCTRL_Mode(uint8_t newmode) {
    // If the chip is asleep and we want to change mode then a manual wake needs to be done
    // This is done by setting the wake up interrupt flag
    // This undocumented trick was found at https://github.com/mkleemann/can/blob/master/can_sleep_mcp2515.c
    if ((getMode()) == MODE_SLEEP && newmode != MODE_SLEEP) {
        printf("was sleed\n");
        // Make sure wake interrupt is enabled
        uint8_t wakeIntEnabled = (mcp2515_readRegister(MCP_CANINTE) & MCP_WAKIF);
        if (!wakeIntEnabled) {
            mcp2515_modifyRegister(MCP_CANINTE, MCP_WAKIF, MCP_WAKIF);
        }
        // Set wake flag (this does the actual waking up)
        mcp2515_modifyRegister(MCP_CANINTF, MCP_WAKIF, MCP_WAKIF);
        // Wait for the chip to exit SLEEP and enter LISTENONLY mode.
        // If the chip is not connected to a CAN bus (or the bus has no other powered nodes) it will sometimes trigger the wake interrupt as soon
        // as it's put to sleep, but it will stay in SLEEP mode instead of automatically switching to LISTENONLY mode.
        // In this situation the mode needs to be manually set to LISTENONLY.
        if (mcp2515_requestNewMode(MODE_LISTENONLY) != MCP2515_OK) {
            return MCP2515_FAIL;
        }
        // Turn wake interrupt back off if it was originally off
        if (!wakeIntEnabled) {
            mcp2515_modifyRegister(MCP_CANINTE, MCP_WAKIF, 0);
        }
    }

    // Clear wake flag
    mcp2515_modifyRegister(MCP_CANINTF, MCP_WAKIF, 0);
    printf("set new mode\n");
    return mcp2515_requestNewMode(newmode);
}

uint8_t mcp2515_readRegister(uint8_t address) {
    uint8_t ret;
    MCP2515_SELECT();
    uint8_t TX_data[2];
    *TX_data = MCP_READ;
    *(TX_data + 1) = address;
//    spi_write_blocking(SPI_PORT, (const uint8_t *) MCP_READ, 1);
    spi_write_blocking(SPI_PORT, TX_data, 2);
    spi_read_blocking(SPI_PORT, 0, &ret, 1);
    sleep_us(1);
    MCP2515_UNSELECT();
#if DEBUG_EN
//    printf("ret:%02x %02x %02x\n", MCP_READ, address, ret);
#endif
    return ret;
}

uint8_t getMode() {
    return mcp2515_readRegister(MCP_CANSTAT) & MODE_MASK;
}

void mcp2515_modifyRegister(uint8_t address, uint8_t mask, uint8_t data) {
    MCP2515_SELECT();
    uint8_t TX_data[4] = {MCP_BITMOD, address, mask, data};
    spi_write_blocking(SPI_PORT, TX_data, 4);
//    printf("mode set: %02x %02x %02x %02x\n", TX_data[0], TX_data[1], TX_data[2], TX_data[3]);
    MCP2515_UNSELECT();
}

uint8_t mcp2515_requestNewMode(uint8_t newmode) {
    unsigned long startTime = to_ms_since_boot(get_absolute_time());

    // Spam new mode request and wait for the operation  to complete
    while (1) {
        // Request new mode
        // This is inside the loop as sometimes requesting the new mode once doesn't work (usually when attempting to sleep)
        mcp2515_modifyRegister(MCP_CANCTRL, MODE_MASK, newmode);
        sleep_ms(20);
        uint8_t statReg = mcp2515_readRegister(MCP_CANSTAT);
        if ((statReg & MODE_MASK) == newmode) { // We're now in the new mode
            return MCP2515_OK;
        } else if ((to_ms_since_boot(get_absolute_time()) - startTime) >
                   200) { // Wait no more than 200ms for the operation to complete
//            return MCP2515_FAIL;
        }
    }
}

uint8_t mcp2515_configRate(uint16_t canSpeed, uint8_t clock) {
    uint8_t set, cfg1, cfg2, cfg3;
    set = 1;
    switch (clock) {
        case (MCP_16MHz) :
            switch (canSpeed) {
                case (CAN_5KBPS):
                    cfg1 = MCP_16MHz_5kBPS_CFG1;
                    cfg2 = MCP_16MHz_5kBPS_CFG2;
                    cfg3 = MCP_16MHz_5kBPS_CFG3;
                    break;

                case (CAN_10KBPS):
                    cfg1 = MCP_16MHz_10kBPS_CFG1;
                    cfg2 = MCP_16MHz_10kBPS_CFG2;
                    cfg3 = MCP_16MHz_10kBPS_CFG3;
                    break;
                case (CAN_20KBPS):
                    cfg1 = MCP_16MHz_20kBPS_CFG1;
                    cfg2 = MCP_16MHz_20kBPS_CFG2;
                    cfg3 = MCP_16MHz_20kBPS_CFG3;
                    break;

                case (CAN_25KBPS):
                    cfg1 = MCP_16MHz_25kBPS_CFG1;
                    cfg2 = MCP_16MHz_25kBPS_CFG2;
                    cfg3 = MCP_16MHz_25kBPS_CFG3;
                    break;

                case (CAN_31K25BPS):
                    cfg1 = MCP_16MHz_31k25BPS_CFG1;
                    cfg2 = MCP_16MHz_31k25BPS_CFG2;
                    cfg3 = MCP_16MHz_31k25BPS_CFG3;
                    break;

                case (CAN_33KBPS):
                    cfg1 = MCP_16MHz_33kBPS_CFG1;
                    cfg2 = MCP_16MHz_33kBPS_CFG2;
                    cfg3 = MCP_16MHz_33kBPS_CFG3;
                    break;

                case (CAN_40KBPS):
                    cfg1 = MCP_16MHz_40kBPS_CFG1;
                    cfg2 = MCP_16MHz_40kBPS_CFG2;
                    cfg3 = MCP_16MHz_40kBPS_CFG3;
                    break;

                case (CAN_50KBPS):
                    cfg1 = MCP_16MHz_50kBPS_CFG1;
                    cfg2 = MCP_16MHz_50kBPS_CFG2;
                    cfg3 = MCP_16MHz_50kBPS_CFG3;
                    break;

                case (CAN_80KBPS):
                    cfg1 = MCP_16MHz_80kBPS_CFG1;
                    cfg2 = MCP_16MHz_80kBPS_CFG2;
                    cfg3 = MCP_16MHz_80kBPS_CFG3;
                    break;

                case (CAN_83K3BPS):
                    cfg1 = MCP_16MHz_83k3BPS_CFG1;
                    cfg2 = MCP_16MHz_83k3BPS_CFG2;
                    cfg3 = MCP_16MHz_83k3BPS_CFG3;
                    break;

                case (CAN_95KBPS):
                    cfg1 = MCP_16MHz_95kBPS_CFG1;
                    cfg2 = MCP_16MHz_95kBPS_CFG2;
                    cfg3 = MCP_16MHz_95kBPS_CFG3;
                    break;

                case (CAN_100KBPS):
                    cfg1 = MCP_16MHz_100kBPS_CFG1;
                    cfg2 = MCP_16MHz_100kBPS_CFG2;
                    cfg3 = MCP_16MHz_100kBPS_CFG3;
                    break;

                case (CAN_125KBPS):
                    cfg1 = MCP_16MHz_125kBPS_CFG1;
                    cfg2 = MCP_16MHz_125kBPS_CFG2;
                    cfg3 = MCP_16MHz_125kBPS_CFG3;
                    break;

                case (CAN_200KBPS):
                    cfg1 = MCP_16MHz_200kBPS_CFG1;
                    cfg2 = MCP_16MHz_200kBPS_CFG2;
                    cfg3 = MCP_16MHz_200kBPS_CFG3;
                    break;

                case (CAN_250KBPS):
                    cfg1 = MCP_16MHz_250kBPS_CFG1;
                    cfg2 = MCP_16MHz_250kBPS_CFG2;
                    cfg3 = MCP_16MHz_250kBPS_CFG3;
                    break;

                case (CAN_500KBPS):
                    cfg1 = MCP_16MHz_500kBPS_CFG1;
                    cfg2 = MCP_16MHz_500kBPS_CFG2;
                    cfg3 = MCP_16MHz_500kBPS_CFG3;
                    break;

                case (CAN_666KBPS):
                    cfg1 = MCP_16MHz_666kBPS_CFG1;
                    cfg2 = MCP_16MHz_666kBPS_CFG2;
                    cfg3 = MCP_16MHz_666kBPS_CFG3;
                    break;

                case (CAN_800KBPS) :
                    cfg1 = MCP_16MHz_800kBPS_CFG1;
                    cfg2 = MCP_16MHz_800kBPS_CFG2;
                    cfg3 = MCP_16MHz_800kBPS_CFG3;
                    break;

                case (CAN_1000KBPS):
                    cfg1 = MCP_16MHz_1000kBPS_CFG1;
                    cfg2 = MCP_16MHz_1000kBPS_CFG2;
                    cfg3 = MCP_16MHz_1000kBPS_CFG3;
                    break;

                default:
                    set = 0;
                    break;
            }
            break;

        case (MCP_12MHz) :
            switch (canSpeed) {
                case (CAN_20KBPS) :
                    cfg1 = MCP_12MHz_20kBPS_CFG1;
                    cfg2 = MCP_12MHz_20kBPS_CFG2;
                    cfg3 = MCP_12MHz_20kBPS_CFG3;
                    break;

                case (CAN_25KBPS) :
                    cfg1 = MCP_12MHz_25kBPS_CFG1;
                    cfg2 = MCP_12MHz_25kBPS_CFG2;
                    cfg3 = MCP_12MHz_25kBPS_CFG3;
                    break;

                case (CAN_31K25BPS) :
                    cfg1 = MCP_12MHz_31k25BPS_CFG1;
                    cfg2 = MCP_12MHz_31k25BPS_CFG2;
                    cfg3 = MCP_12MHz_31k25BPS_CFG3;
                    break;

                case (CAN_33KBPS) :
                    cfg1 = MCP_12MHz_33kBPS_CFG1;
                    cfg2 = MCP_12MHz_33kBPS_CFG2;
                    cfg3 = MCP_12MHz_33kBPS_CFG3;
                    break;

                case (CAN_40KBPS) :
                    cfg1 = MCP_12MHz_40kBPS_CFG1;
                    cfg2 = MCP_12MHz_40kBPS_CFG2;
                    cfg3 = MCP_12MHz_40kBPS_CFG3;
                    break;

                case (CAN_50KBPS) :
                    cfg1 = MCP_12MHz_50kBPS_CFG1;
                    cfg2 = MCP_12MHz_50kBPS_CFG2;
                    cfg3 = MCP_12MHz_50kBPS_CFG3;
                    break;

                case (CAN_80KBPS) :
                    cfg1 = MCP_12MHz_80kBPS_CFG1;
                    cfg2 = MCP_12MHz_80kBPS_CFG2;
                    cfg3 = MCP_12MHz_80kBPS_CFG3;
                    break;

                case (CAN_83K3BPS) :
                    cfg1 = MCP_12MHz_83k3BPS_CFG1;
                    cfg2 = MCP_12MHz_83k3BPS_CFG2;
                    cfg3 = MCP_12MHz_83k3BPS_CFG3;
                    break;

                case (CAN_95KBPS) :
                    cfg1 = MCP_12MHz_95kBPS_CFG1;
                    cfg2 = MCP_12MHz_95kBPS_CFG2;
                    cfg3 = MCP_12MHz_95kBPS_CFG3;
                    break;

                case (CAN_100KBPS) :
                    cfg1 = MCP_12MHz_100kBPS_CFG1;
                    cfg2 = MCP_12MHz_100kBPS_CFG2;
                    cfg3 = MCP_12MHz_100kBPS_CFG3;
                    break;

                case (CAN_125KBPS) :
                    cfg1 = MCP_12MHz_125kBPS_CFG1;
                    cfg2 = MCP_12MHz_125kBPS_CFG2;
                    cfg3 = MCP_12MHz_125kBPS_CFG3;
                    break;

                case (CAN_200KBPS) :
                    cfg1 = MCP_12MHz_200kBPS_CFG1;
                    cfg2 = MCP_12MHz_200kBPS_CFG2;
                    cfg3 = MCP_12MHz_200kBPS_CFG3;
                    break;

                case (CAN_250KBPS) :
                    cfg1 = MCP_12MHz_250kBPS_CFG1;
                    cfg2 = MCP_12MHz_250kBPS_CFG2;
                    cfg3 = MCP_12MHz_250kBPS_CFG3;
                    break;

                case (CAN_500KBPS) :
                    cfg1 = MCP_12MHz_500kBPS_CFG1;
                    cfg2 = MCP_12MHz_500kBPS_CFG2;
                    cfg3 = MCP_12MHz_500kBPS_CFG3;
                    break;

                case (CAN_666KBPS) :
                    cfg1 = MCP_12MHz_666kBPS_CFG1;
                    cfg2 = MCP_12MHz_666kBPS_CFG2;
                    cfg3 = MCP_12MHz_666kBPS_CFG3;
                    break;

                case (CAN_1000KBPS) :
                    cfg1 = MCP_12MHz_1000kBPS_CFG1;
                    cfg2 = MCP_12MHz_1000kBPS_CFG2;
                    cfg3 = MCP_12MHz_1000kBPS_CFG3;
                    break;

                default:
                    set = 0;
                    break;
            }
            break;
        case (MCP_8MHz) :
            switch (canSpeed) {
                case (CAN_5KBPS) :
                    cfg1 = MCP_8MHz_5kBPS_CFG1;
                    cfg2 = MCP_8MHz_5kBPS_CFG2;
                    cfg3 = MCP_8MHz_5kBPS_CFG3;
                    break;

                case (CAN_10KBPS) :
                    cfg1 = MCP_8MHz_10kBPS_CFG1;
                    cfg2 = MCP_8MHz_10kBPS_CFG2;
                    cfg3 = MCP_8MHz_10kBPS_CFG3;
                    break;

                case (CAN_20KBPS) :
                    cfg1 = MCP_8MHz_20kBPS_CFG1;
                    cfg2 = MCP_8MHz_20kBPS_CFG2;
                    cfg3 = MCP_8MHz_20kBPS_CFG3;
                    break;

                case (CAN_31K25BPS) :
                    cfg1 = MCP_8MHz_31k25BPS_CFG1;
                    cfg2 = MCP_8MHz_31k25BPS_CFG2;
                    cfg3 = MCP_8MHz_31k25BPS_CFG3;
                    break;

                case (CAN_40KBPS) :
                    cfg1 = MCP_8MHz_40kBPS_CFG1;
                    cfg2 = MCP_8MHz_40kBPS_CFG2;
                    cfg3 = MCP_8MHz_40kBPS_CFG3;
                    break;

                case (CAN_50KBPS) :
                    cfg1 = MCP_8MHz_50kBPS_CFG1;
                    cfg2 = MCP_8MHz_50kBPS_CFG2;
                    cfg3 = MCP_8MHz_50kBPS_CFG3;
                    break;

                case (CAN_80KBPS) :
                    cfg1 = MCP_8MHz_80kBPS_CFG1;
                    cfg2 = MCP_8MHz_80kBPS_CFG2;
                    cfg3 = MCP_8MHz_80kBPS_CFG3;
                    break;

                case (CAN_100KBPS) :
                    cfg1 = MCP_8MHz_100kBPS_CFG1;
                    cfg2 = MCP_8MHz_100kBPS_CFG2;
                    cfg3 = MCP_8MHz_100kBPS_CFG3;
                    break;

                case (CAN_125KBPS) :
                    cfg1 = MCP_8MHz_125kBPS_CFG1;
                    cfg2 = MCP_8MHz_125kBPS_CFG2;
                    cfg3 = MCP_8MHz_125kBPS_CFG3;
                    break;

                case (CAN_200KBPS) :
                    cfg1 = MCP_8MHz_200kBPS_CFG1;
                    cfg2 = MCP_8MHz_200kBPS_CFG2;
                    cfg3 = MCP_8MHz_200kBPS_CFG3;
                    break;

                case (CAN_250KBPS) :
                    cfg1 = MCP_8MHz_250kBPS_CFG1;
                    cfg2 = MCP_8MHz_250kBPS_CFG2;
                    cfg3 = MCP_8MHz_250kBPS_CFG3;
                    break;

                case (CAN_500KBPS) :
                    cfg1 = MCP_8MHz_500kBPS_CFG1;
                    cfg2 = MCP_8MHz_500kBPS_CFG2;
                    cfg3 = MCP_8MHz_500kBPS_CFG3;
                    break;

                case (CAN_800KBPS) :
                    cfg1 = MCP_8MHz_800kBPS_CFG1;
                    cfg2 = MCP_8MHz_800kBPS_CFG2;
                    cfg3 = MCP_8MHz_800kBPS_CFG3;
                    break;

                case (CAN_1000KBPS) :
                    cfg1 = MCP_8MHz_1000kBPS_CFG1;
                    cfg2 = MCP_8MHz_1000kBPS_CFG2;
                    cfg3 = MCP_8MHz_1000kBPS_CFG3;
                    break;

                default:
                    set = 0;
                    break;
            }
            break;

        default:
            set = 0;
            break;
    }

    if (set) {
        mcp2515_setRegister(MCP_CNF1, cfg1);
        mcp2515_setRegister(MCP_CNF2, cfg2);
        mcp2515_setRegister(MCP_CNF3, cfg3);
        return MCP2515_OK;
    } else {
        return MCP2515_FAIL;
    }
}

void mcp2515_setRegister(uint8_t address, uint8_t value) {
    MCP2515_SELECT();
    uint8_t TX_data[3] = {MCP_WRITE, address, value};
    spi_write_blocking(SPI_PORT, TX_data, 3);
    MCP2515_UNSELECT();
}

void mcp2515_initCANBuffers(void) {
    uint8_t i, a1, a2, a3;

    a1 = MCP_TXB0CTRL;
    a2 = MCP_TXB1CTRL;
    a3 = MCP_TXB2CTRL;
    for (i = 0; i < 14; i++) {                       // in-buffer loop
        mcp2515_setRegister(a1, 0);
        mcp2515_setRegister(a2, 0);
        mcp2515_setRegister(a3, 0);
        a1++;
        a2++;
        a3++;
    }
    mcp2515_setRegister(MCP_RXB0CTRL, 0);
    mcp2515_setRegister(MCP_RXB1CTRL, 0);
}

uint8_t setMode(uint8_t opMode) {
    // if going to sleep, the value stored in opMode is not changed so that we can return to it later
    if (opMode != MODE_SLEEP) {
        mcpMode = opMode;
    }
    return mcp2515_setCANCTRL_Mode(opMode);
}

uint8_t init_Mask(uint8_t num, uint8_t ext, unsigned long ulData) {
    uint8_t res = MCP2515_OK;
#if DEBUG_EN
    printf("Begin to set Mask!!\n");
#else
    sleep_us(10);
#endif
    res = mcp2515_setCANCTRL_Mode(MODE_CONFIG);
    if (res > 0) {
#if DEBUG_EN
        printf("Enter setting mode fall\n");
#else
        sleep_us(10);
#endif
        return res;
    }

    if (num == 0) {
        mcp2515_write_id(MCP_RXM0SIDH, ext, ulData);

    } else if (num == 1) {
        mcp2515_write_id(MCP_RXM1SIDH, ext, ulData);
    } else {
        res = MCP2515_FAIL;
    }
    res = mcp2515_setCANCTRL_Mode(mcpMode);
    if (res > 0) {
#if DEBUG_EN
        printf("Enter normal mode fall\n");
#else
        sleep_us(10);
#endif
        return res;
    }
#if DEBUG_EN
    printf("set Mask success!!\n");
#else
    sleep_us(10);
#endif
    return res;
}

void mcp2515_write_id(uint8_t mcp_addr, uint8_t ext, unsigned long id) {
    uint8_t tbufdata[4];
    mcp2515_id_to_buf(ext, id, tbufdata);
    mcp2515_setRegisterS(mcp_addr, tbufdata, 4);
}

void mcp2515_id_to_buf(uint8_t ext, unsigned long id, uint8_t *tbufdata) {
    uint16_t canid;

    canid = (uint16_t) (id & 0x0FFFF);
//    printf("canid:%04x\n", canid);
    if (ext == 1) {
        tbufdata[MCP_EID0] = (uint8_t) (canid & 0xFF);
        tbufdata[MCP_EID8] = (uint8_t) (canid >> 8);
        canid = (uint16_t) (id >> 16);
        tbufdata[MCP_SIDL] = (uint8_t) (canid & 0x03);
        tbufdata[MCP_SIDL] += (uint8_t) ((canid & 0x1C) << 3);
        tbufdata[MCP_SIDL] |= MCP_TXB_EXIDE_M;
        tbufdata[MCP_SIDH] = (uint8_t) (canid >> 5);
    } else {
        tbufdata[MCP_SIDH] = (uint8_t) (canid >> 3);
        tbufdata[MCP_SIDL] = (uint8_t) ((canid & 0x07) << 5);
        tbufdata[MCP_EID0] = 0;
        tbufdata[MCP_EID8] = 0;
    }
}

void mcp2515_setRegisterS(uint8_t address, uint8_t values[], uint8_t len) {
    MCP2515_SELECT();
    printf("%02x %02x %02x %02x %02x %02x\n", MCP_WRITE, address, values[0], values[1], values[2], values[3]);
    uint8_t write_mode = MCP_WRITE;
    spi_write_blocking(SPI_PORT, &write_mode, 1);
    spi_write_blocking(SPI_PORT, &address, 1);
    spi_write_blocking(SPI_PORT, values, 1);
    sleep_us(1);
    MCP2515_UNSELECT();
}

uint8_t init_Filt(uint8_t num, uint8_t ext, unsigned long ulData) {
    uint8_t res = MCP2515_OK;
#if DEBUG_EN
    printf("Begin to set Filter!!\n");
#else
    sleep_us(10);
#endif
    res = mcp2515_setCANCTRL_Mode(MODE_CONFIG);
    if (res > 0) {
#if DEBUG_EN
        printf("Enter setting mode fall\n");
#else
        sleep_us(10);
#endif
        return res;
    }

    switch (num) {
        case 0:
//            printf("ulData:%08lx\n", ulData);
            mcp2515_write_id(MCP_RXF0SIDH, ext, ulData);
            break;

        case 1:
            mcp2515_write_id(MCP_RXF1SIDH, ext, ulData);
            break;

        case 2:
            mcp2515_write_id(MCP_RXF2SIDH, ext, ulData);
            break;

        case 3:
            mcp2515_write_id(MCP_RXF3SIDH, ext, ulData);
            break;

        case 4:
            mcp2515_write_id(MCP_RXF4SIDH, ext, ulData);
            break;

        case 5:
            mcp2515_write_id(MCP_RXF5SIDH, ext, ulData);
            break;

        default:
            res = MCP2515_FAIL;
    }

    res = mcp2515_setCANCTRL_Mode(mcpMode);
    if (res > 0) {
#if DEBUG_EN
        printf("Enter normal mode fall\r\nSet filter fail!!\n");
#else
        sleep_us(10);
#endif
        return res;
    }
#if DEBUG_EN
    printf("set Filter success!!\n");
#else
    sleep_us(10);
#endif

    return res;
}

void mcp2515_read_canMsg(uint8_t buffer_load_addr, volatile unsigned long *id, volatile uint8_t *ext,
                         volatile uint8_t *rtrBit, volatile uint8_t *len,
                         volatile uint8_t *buf) {      /* read can msg                 */
    uint8_t tbufdata[4];

    MCP2515_SELECT();
    spi_write_blocking(SPI_PORT, &buffer_load_addr, 1);
    // mcp2515 has auto-increment of address-pointer
    spi_read_blocking(SPI_PORT, 0, tbufdata, 4);
//    printf("%x %x %x %x\n", tbufdata[0], tbufdata[1], tbufdata[2], tbufdata[3]);
    *id = (tbufdata[MCP_SIDH] << 3) + (tbufdata[MCP_SIDL] >> 5);
    *ext = 0;
    if ((tbufdata[MCP_SIDL] & MCP_TXB_EXIDE_M) == MCP_TXB_EXIDE_M) {
        /* extended id                  */
        *id = (*id << 2) + (tbufdata[MCP_SIDL] & 0x03);
        *id = (*id << 8) + tbufdata[MCP_EID8];
        *id = (*id << 8) + tbufdata[MCP_EID0];
        *ext = 1;
    }

    uint8_t pMsgSize;
    spi_read_blocking(SPI_PORT, 0, &pMsgSize, 1);
    *len = pMsgSize & MCP_DLC_MASK;
    *rtrBit = (pMsgSize & MCP_RTR_MASK) ? 1 : 0;
    spi_read_blocking(SPI_PORT, 0, buf, *len);

    mcp2515_modifyRegister(MCP_EFLG, MCP_EFLG_RX0OVR, 0);

    MCP2515_UNSELECT();
}

void mcp2515_write_canMsg(uint8_t buffer_sidh_addr, unsigned long id, uint8_t ext, uint8_t rtrBit, uint8_t len,
                          volatile uint8_t *buf) {
    uint8_t load_addr = txSidhToTxLoad(buffer_sidh_addr);

    uint8_t tbufdata[4];
    uint8_t dlc = len | (rtrBit ? MCP_RTR_MASK : 0);

    mcp2515_id_to_buf(ext, id, tbufdata);

//    printf("send_data:%02x %02x %02x %02x\n", tbufdata[0], tbufdata[1], tbufdata[2], tbufdata[3]);

    MCP2515_SELECT();
    spi_write_blocking(SPI_PORT, &load_addr, 1);
    spi_write_blocking(SPI_PORT, tbufdata, 4);
    spi_write_blocking(SPI_PORT, &dlc, 1);
    spi_write_blocking(SPI_PORT, buf, len);
    MCP2515_UNSELECT();

    mcp2515_start_transmit(buffer_sidh_addr);
}

uint8_t txSidhToRTS(uint8_t sidh) {
    switch (sidh) {
        case MCP_TXB0SIDH:
            return MCP_RTS_TX0;
        case MCP_TXB1SIDH:
            return MCP_RTS_TX1;
        case MCP_TXB2SIDH:
            return MCP_RTS_TX2;
    }
    return 0;
}

uint8_t txSidhToTxLoad(uint8_t sidh) {
    switch (sidh) {
        case MCP_TXB0SIDH:
            return MCP_LOAD_TX0;
        case MCP_TXB1SIDH:
            return MCP_LOAD_TX1;
        case MCP_TXB2SIDH:
            return MCP_LOAD_TX2;
    }
    return 0;
}

void mcp2515_start_transmit(uint8_t mcp_addr) {            // start transmit
    MCP2515_SELECT();
    uint8_t send_buf = txSidhToRTS(mcp_addr);
    spi_write_blocking(SPI_PORT, &send_buf, 1);
    MCP2515_UNSELECT();
}